import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node


def generate_launch_description():

    # 获取机器人的share目录路径
    bringup_dir = get_package_share_directory('robot_bringup')
    config_dir = os.path.join(bringup_dir, 'config')

    # 参数文件路径
    params_file = os.path.join(config_dir, 'shared_params.yaml')

    ################################################################################
    # mower_description
    ################################################################################

    # 机器描述文件
    robot_type = IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(bringup_dir,'launch','robot_mode_description.launch.py')),
            launch_arguments={'brushless_senior_diff': 'true'}.items(),
    )

    ################################################################################
    # hardware_module
    ################################################################################

    # 底盘(hardware_interface)
    hardware_interface_dir = get_package_share_directory('hardware_interface')
    hardware_interface = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(hardware_interface_dir, 'launch','hardware_interface.launch.py')),
    )

    #IMU 惯导 20241031-dw.liu
    imu_ahrs_dir = get_package_share_directory('imu_driver')
    imu_ahrs = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(imu_ahrs_dir,'launch','imu_driver.launch.py')),
    )

    # GPS
    rtk_fast30_dir = get_package_share_directory('rtk_fast30')
    rtk_fast30_driver = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(rtk_fast30_dir,'launch','launch_driver.launch.py')),
    )

    gps_driver_dir = get_package_share_directory('gps_driver')
    # print(f"nmea_navsat_driver_share_dir: ", gps_driver_dir)
    gps_driver = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(gps_driver_dir,'launch','gps_driver.launch.py')),
    )

    # 雷达
    # Lslidar_dir = get_package_share_directory('lslidar_driver')
    # Lsm10_m10p_uart = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(os.path.join(Lslidar_dir, 'launch','lsm10p_uart_launch.py')),)
    
    ################################################################################
    # calibration
    ################################################################################

    # IMU 标定与修正
    imu_calibration_dir = get_package_share_directory('imu_cc')
    imu_calibration = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(imu_calibration_dir,'launch','imu_cc.launch.py')),
    )


    ################################################################################
    # nav_module
    ################################################################################

    # 传感器数据融合
    # robot_localization_node_share_dir = get_package_share_directory('robot_localization_node')
    # robot_localization_node = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(os.path.join(robot_localization_node_share_dir,'launch','robot_localization_node.launch.py')),
    #     #PythonLaunchDescriptionSource(os.path.join(robot_localization_node_share_dir,'launch','robot_localization_1.launch.py')),
    # )

    ################################################################################
    # map_module
    ################################################################################

    # 地图模块
    # map_module_dir = get_package_share_directory('map_module')
    # map_module = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(os.path.join(map_module_dir,'launch','map_server.launch.py')),
    #     # launch_arguments={'params_file': os.path.join(map_module_dir, 'config', 'params.yaml')}.items(),
    #     launch_arguments={'params_file': params_file}.items()
    # )

    ################################################################################
    # 路径生成
    ################################################################################
    # global_plan_path_dir = get_package_share_directory('global_plan_path')
    # global_plan_path_node = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(os.path.join(global_plan_path_dir,'launch','global_plan_path.launch.py')),
    # )

    ################################################################################
    # 运动控制
    ################################################################################
    # dynamic_controller_dir = get_package_share_directory('dynamic_controller')
    # dynamic_controller_node = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(os.path.join(dynamic_controller_dir,'launch','dynamic_controller.launch.py')),
    # )

    #-------------------------#

    # 创建LaunchDescription对象，并添加电机驱动节点和传感器launch文件的动作
    ld = LaunchDescription()
    ld.add_action(robot_type)

    ld.add_action(hardware_interface)
    ld.add_action(imu_ahrs)  
    ld.add_action(rtk_fast30_driver)
    # ld.add_action(gps_driver)
    # ld.add_action(Lsm10_m10p_uart) # 暂时用不到雷达

    ld.add_action(imu_calibration)

    # ld.add_action(robot_localization_node)
    # ld.add_action(map_module)
    # ld.add_action(global_plan_path_node)
    # ld.add_action(dynamic_controller_node)
    
    return ld


